#define pi 3.1415926

#include <Servo.h>        //声明调用Servo.h库
#include <winbondflash.h> //flash调用的库
#include <SoftwareSerial.h>

#define ACTION_SIZE 512
#define RECV_SIZE 168
#define INFO_ADDR_SAVE_STR (((8 << 10) - 4) << 10) //(8*1024-4)*1024    //eeprom_info结构体存储的位置
#define BIAS_ADDR_VERIFY 0                         // 偏差存储的地址
#define FLAG_VERIFY 0x38

#define PIN_nled 13 // 宏定义工作指示灯引脚
#define PIN_beep 4  // 蜂鸣器引脚定义

#define nled_on()                \
  {                              \
    digitalWrite(PIN_nled, LOW); \
  }
#define nled_off()                \
  {                               \
    digitalWrite(PIN_nled, HIGH); \
  }

#define beep_on()                 \
  {                               \
    digitalWrite(PIN_beep, HIGH); \
  }
#define beep_off()               \
  {                              \
    digitalWrite(PIN_beep, LOW); \
  }

#define PRE_CMD_SIZE 128
#define SERVO_NUM 6
#define SERVO_TIME_PERIOD 20 // 每隔20ms处理一次（累加）舵机的PWM增量

typedef struct
{
  long myversion;
  long dj_record_num;
  byte pre_cmd[PRE_CMD_SIZE + 1];
  int dj_bias_pwm[SERVO_NUM + 1];
} eeprom_info_t;
eeprom_info_t eeprom_info;

Servo myservo[SERVO_NUM]; // 创建一个舵机类
// char buffer[RECV_SIZE];                 // 定义一个数组用来存储每小组动作组
byte uart_receive_buf[RECV_SIZE];
byte servo_pin[SERVO_NUM] = {7, 3, 5, 6, 9, 8}; // 宏定义舵机控制引脚
String uart_receive_str = "";                   // 声明一个字符串数组
String uart_receive_str_bak = "";               // 声明一个字符串数组
byte uart1_get_ok = 0, uart1_mode = 0;
char cmd_return[64];
int uart_receive_str_len;
int zx_read_id = 0, zx_read_flag = 0, zx_read_value = 0;
byte flag_sync = 0;
bool downLoad = false;
u32 downLoadSystickMs = 0;

winbondFlashSPI mem;
int do_start_index, do_time, group_num_start, group_num_end, group_num_times;
char group_do_ok = 1;
long long action_time = 0;
void (*resetFunc)(void) = 0; // declare reset function at address 0

// 变量定义
#define MOTOR_NUM 4
#define MOTOR_PWM_MAX 2500
#define MOTOR_PWM_MIN 500
#define MOTOR_PWM_INIT 1500
#define PID_MOTOR_PULSE_PRECISION 5
#define MOTOR_PWM_SEND_PERIOD 15 // 发送电机PWM信号周期

int motors_pwm[MOTOR_NUM] = {1500, 1500, 1500, 1500}; // 电机pwm信号
int motors_current[MOTOR_NUM] = {0, 0, 0, 0};         // 电机当前脉冲值
int motors_target[MOTOR_NUM] = {0, 0, 0, 0};          // 电机目标脉冲值
int motors_direction[MOTOR_NUM] = {0, 0, 0, 0};       // 电机方向
const int motors_id[MOTOR_NUM] = {7, 6, 9, 8};        // 电机ID

double pid_motor_Kp[MOTOR_NUM] = {0.5, 0.25, 0.25, 0.25};
double pid_motor_Ki[MOTOR_NUM] = {0.01, 0.005, 0.005, 0.005};
double pid_motor_Kd[MOTOR_NUM] = {1,1,1,1};
double pid_motor_pre_error[MOTOR_NUM] = {0, 0, 0, 0};
double pid_motor_error[MOTOR_NUM] = {0, 0, 0, 0};
double pid_motor_output[MOTOR_NUM] = {0, 0, 0, 0};
double pid_motor_integral[MOTOR_NUM] = {0, 0, 0, 0};

// 软串口初始化
SoftwareSerial Serial_Esp32(7, 3);
byte uart2_get_ok = 0, uart2_mode = 0;
byte uart2_receive_buf[RECV_SIZE];
String uart2_receive_str = "";
int uart2_receive_str_len;

// 电机PID控制函数
void pid_motor_control(int motor_id)
{
  char cmd_tmp[64];
  bool isChangePWM = true;
  pid_motor_error[motor_id] = motors_target[motor_id] - motors_current[motor_id];
  if (abs(pid_motor_error[motor_id]) < PID_MOTOR_PULSE_PRECISION)
  {
    pid_motor_error[motor_id] = 0;
  }
  pid_motor_integral[motor_id] += pid_motor_error[motor_id];
  pid_motor_output[motor_id] = pid_motor_Kp[motor_id] * pid_motor_error[motor_id] +
                               pid_motor_Ki[motor_id] * pid_motor_integral[motor_id] +
                               pid_motor_Kd[motor_id] * (pid_motor_error[motor_id] - pid_motor_pre_error[motor_id]);
  pid_motor_pre_error[motor_id] = pid_motor_error[motor_id];
  motors_pwm[motor_id] += pid_motor_output[motor_id];
  if (pid_motor_output[motor_id] != 0)
  {
    isChangePWM = true;
  }
  if (motors_pwm[motor_id] > MOTOR_PWM_MAX)
  {
    motors_pwm[motor_id] = MOTOR_PWM_MAX;
  }
  else if (motors_pwm[motor_id] < MOTOR_PWM_MIN)
  {
    motors_pwm[motor_id] = MOTOR_PWM_MIN;
  }
  // motors_pwm[motor_id] = pid_motor_output[motor_id];
  if (isChangePWM)
  {
    // sprintf((char *)cmd_tmp, "#%03dP%04dT0200!", motors_id[motor_id], motors_pwm[motor_id]);
    // Serial.print(cmd_tmp);
    set_motor_pwm(motor_id, motors_pwm[motor_id]);
  }
}

// 设置电机目标脉冲值
void set_motor_target(int motor_id, int target)
{
  motors_target[motor_id] = target;
}

// 设置电机PID参数
void set_motor_pid(int motor_id, double kp, double ki, double kd)
{
  pid_motor_Kp[motor_id] = kp;
  pid_motor_Ki[motor_id] = ki;
  pid_motor_Kd[motor_id] = kd;
}

// 发送电机控制函数
void send_motor_pwm()
{
  char cmd_tmp[64];
  sprintf(cmd_tmp, "{#%03dP%04dT1000!#%03dP%04dT1000!#%03dP%04dT1000!#%03dP%04dT1000!}", motors_id[0], motors_pwm[0], motors_id[1], motors_pwm[1], motors_id[2], motors_pwm[2], motors_id[3], motors_pwm[3]);
  Serial.println(cmd_tmp);
}

// 发送动作指令舵机角度
void set_servo(int mindex, int mpwm, int mtime)
{
  sprintf((char *)cmd_return, "#%03dP%04dT%04d! ", mindex, mpwm, mtime);
  Serial.print(cmd_return);
  // Serial.println(kinematics.servo_angle[mindex]);
}

// 更新电机PWM值
void set_motor_pwm(int motor_id, int pwm)
{
  char cmd_tmp[64];
  motors_pwm[motor_id] = pwm;
  sprintf(cmd_tmp, "#%03dP%04dT0000!", motors_id[motor_id], motors_pwm[motor_id]);
  Serial.print(cmd_tmp);
  // 设置电机方向
  if (pwm > 1500)
  {
    motors_direction[motor_id] = 1;
  }
  else if (pwm < 1500)
  {
    motors_direction[motor_id] = -1;
  }
  else
  {
    motors_direction[motor_id] = 0;
  }
}

// 设置电机当前脉冲值
void set_motor_pulse(int motor_id, int pulse)
{
  if (motors_id[0] == motor_id)
  {
    motors_current[0] = pulse*motors_direction[0];
  }
  else if (motors_id[1] == motor_id)
  {
    motors_current[1] = pulse*motors_direction[1];
  }
  else if (motors_id[2] == motor_id)
  {
    motors_current[2] = pulse*motors_direction[2];
  }
  else if (motors_id[3] == motor_id)
  {
    motors_current[3] = pulse*motors_direction[3];
  }
}

void set_motor_tag_pulse(int motor_id, int pulse)
{
  motors_target[motor_id] = pulse;
}

// 对 a 数进行排序
void selection_sort(int *a, int len)
{
  int i, j, mi, t;
  for (i = 0; i < len - 1; i++)
  {
    mi = i;
    for (j = i + 1; j < len; j++)
    {
      if (a[mi] > a[j])
      {
        mi = j;
      }
    }
    if (mi != i)
    {
      t = a[mi];
      a[mi] = a[i];
      a[i] = t;
    }
  }
}

// 查询str是包含str2，并返回最后一个字符所在str的位置
u16 str_contain_str(u8 *str, u8 *str2)
{
  u8 *str_temp, *str_temp2;
  str_temp = str;
  str_temp2 = str2;
  while (*str_temp)
  {
    if (*str_temp == *str_temp2)
    {
      while (*str_temp2)
      {
        if (*str_temp++ != *str_temp2++)
        {
          str_temp = str_temp - (str_temp2 - str2) + 1;
          str_temp2 = str2;
          break;
        }
      }
      if (!*str_temp2)
      {
        return (str_temp - str);
      }
    }
    else
    {
      str_temp++;
    }
  }
  return 0;
}

// led灯初始化
void setup_nled()
{
  pinMode(PIN_nled, OUTPUT); // 设置引脚为输出模式
  nled_off();
}

// 蜂鸣器初始化
void setup_beep()
{
  pinMode(PIN_beep, OUTPUT);
  beep_off();
}

// 启动提示
void setup_start_pre_cmd()
{
  Serial.println("[FinishInit]");
}

// 功能介绍：LED灯闪烁，每秒闪烁一次
void loop_nled()
{
  static u8 val = 0;
  static unsigned long systick_ms_bak = 0;
  if (millis() - systick_ms_bak > 500)
  {
    systick_ms_bak = millis();
    if (val)
    {
      nled_on();
    }
    else
    {
      nled_off();
    }
    val = !val;
  }
}

// 解析串口接收到的字符串指令
void loop_uart()
{
  if (uart1_get_ok)
  {
    // 如果有同步标志直接返回，让同步函数处理
    if (flag_sync)
      return;
    // 打印字符串，测试的时候可以用
    // 转换成字符串数组
    uart_receive_str.toCharArray((char *)uart_receive_buf, uart_receive_str.length() + 1);
    uart_receive_str_len = uart_receive_str.length();
    if (uart1_mode == 1)
    {
      parse_cmd(uart_receive_buf);
    }
    else if (uart1_mode == 2 || uart1_mode == 3)
    {
      parse_action(uart_receive_buf);
    }
    else if (uart1_mode == 4)
    {
    }
    uart1_get_ok = 0;
    uart1_mode = 0;
    uart_receive_str = "";
  }
  if (uart2_get_ok)
  {
    // 如果有同步标志直接返回，让同步函数处理
    if (flag_sync)
      return;
    // 打印字符串，测试的时候可以用
    // 转换成字符串数组
    uart2_receive_str.toCharArray((char *)uart2_receive_buf, uart2_receive_str.length() + 1);
    if (uart2_mode == 1)
    {
      parse_cmd2(uart2_receive_buf);
    }
    uart2_get_ok = 0;
    uart2_mode = 0;
    uart2_receive_str = "";
  }
  if (millis() - downLoadSystickMs > 3000)
  {
    downLoad = false;
  }
}

// 定时获取电机脉冲值，每MOTOR_PWM_SEND_PERIOD(ms)获取一次
void loop_send_motor_pwm()
{
  static unsigned long systick_ms_bak_motor = 0;
  static unsigned short current_motor_id_bak = 0;
  char cmd_tmp[64];
  if (millis() - systick_ms_bak_motor > MOTOR_PWM_SEND_PERIOD)
  {
    systick_ms_bak_motor = millis();
    sprintf(cmd_tmp, "#%03dPSPD!", motors_id[current_motor_id_bak]);
    Serial.print(cmd_tmp);
    delay(5);
    pid_motor_control(current_motor_id_bak);
    current_motor_id_bak = (current_motor_id_bak + 1) % MOTOR_NUM;
  }
}

// 获取最大时间
int getMaxTime(char *str)
{
  int i = 0, max_time = 0, tmp_time = 0;
  while (str[i])
  {
    if (str[i] == 'T')
    {
      tmp_time = (str[i + 1] - '0') * 1000 + (str[i + 2] - '0') * 100 + (str[i + 3] - '0') * 10 + (str[i + 4] - '0');
      if (tmp_time > max_time)
        max_time = tmp_time;
      i = i + 4;
      continue;
    }
    i++;
  }
  return max_time;
}

// 解析串口动作，发送至控制板
void parse_action(u8 *uart_receive_buf)
{
  // Serial.print("[PARSE_ACTION] ");
  // #006PSPD!
  // 012345678
  if ((uart_receive_buf[0] == '#') && (uart_receive_buf[4] == 'P') && (uart_receive_buf[5] == 'S') && (uart_receive_buf[6] == 'P') && (uart_receive_buf[7] == 'D') && (uart_receive_buf[8] == '!'))
  {
    Serial.print((char *)uart_receive_buf);
  }
  // #006PSPD=0/100ms!
  // 01234567890
  else if ((uart_receive_buf[0] == '#') && (uart_receive_buf[4] == 'P') && (uart_receive_buf[5] == 'S') && (uart_receive_buf[6] == 'P') && (uart_receive_buf[7] == 'D') && (uart_receive_buf[8] == '='))
  {
    // ！！！不会回显！！！
    int spd_motor_id = 0;
    int spd_value = 0;
    sscanf((char *)uart_receive_buf, "#%dPSPD=%d/100ms!", &spd_motor_id, &spd_value);
    set_motor_pulse(spd_motor_id, spd_value);
  }
  // Serial.print((char *)uart_receive_buf);
}

// 解析串口指令
void parse_cmd(u8 *uart_receive_buf)
{
  Serial.print("[PARSE_CMD] ");
  Serial.println((char *)uart_receive_buf);
  u16 pos;
  int int1, int2, int3, int4;
  pos = str_contain_str((char *)uart_receive_buf, "$");
  if (pos = str_contain_str((char *)uart_receive_buf, "$SPD:"), pos)
  {
    // 解析速度指令 $SPD:1500,1500,1500,1500!
    if (sscanf((char *)uart_receive_buf, "$SPD:%d,%d,%d,%d!", &int1, &int2, &int3, &int4))
    {
      if (int1 >= MOTOR_PWM_MIN && int1 <= MOTOR_PWM_MAX && int2 >= MOTOR_PWM_MIN && int2 <= MOTOR_PWM_MAX && int3 >= MOTOR_PWM_MIN && int3 <= MOTOR_PWM_MAX && int4 >= MOTOR_PWM_MIN && int4 <= MOTOR_PWM_MAX)
      {
        set_motor_pwm(0, int1);
        set_motor_pwm(1, int2);
        set_motor_pwm(2, int3);
        set_motor_pwm(3, int4);
      }
    }
  }
  else if (pos = str_contain_str((char *)uart_receive_buf, "$GETTAGSPD"), pos)
  {
    // 解析获取目标速度指令 $GETTAGSPD!
    char response_msg[64];
    sprintf(response_msg, "<GETTAGSPD> %04d %04d %04d %04d", motors_pwm[0], motors_pwm[1], motors_pwm[2], motors_pwm[3]);
    Serial.println(response_msg);
  }
  else if (pos = str_contain_str((char *)uart_receive_buf, "$GETSPD"), pos)
  {
    // 解析获取当前速度指令 $GETSPD!
    char response_msg[64];
    sprintf(response_msg, "<GETSPD> %04d %04d %04d %04d", motors_current[0], motors_current[1], motors_current[2], motors_current[3]);
    Serial.println(response_msg);
  }else if (pos = str_contain_str((char *)uart_receive_buf, "$GETTAG"), pos)
  {
    // 解析获取目标位置指令 $GETTAG!
    char response_msg[64];
    sprintf(response_msg, "<GETTAG> %04d %04d %04d %04d", motors_target[0], motors_target[1], motors_target[2], motors_target[3]);
    Serial.println(response_msg);
  }
}

// 串口中断
void serialEvent()
{
  static char sbuf_bak;
  while (Serial.available())
  { //
    sbuf_bak = char(Serial.read());

    if (uart1_get_ok)
      return;
    if (uart1_mode == 0)
    {
      if (sbuf_bak == '<')
      {
        uart1_mode = 4;
        downLoadSystickMs = millis();
        downLoad = true;
      }
      else if (sbuf_bak == '{')
      {
        uart1_mode = 3;
      }
      else if (sbuf_bak == '$')
      {
        uart1_mode = 1;
      }
      else if (sbuf_bak == '#')
      {
        uart1_mode = 2;
      }
      uart_receive_str = "";
    }

    uart_receive_str += sbuf_bak;

    if ((uart1_mode == 4) && (sbuf_bak == '>'))
    {
      uart1_get_ok = 1;
    }
    else if ((uart1_mode == 3) && (sbuf_bak == '}'))
    {
      uart1_get_ok = 1;
    }
    else if ((uart1_mode == 1) && (sbuf_bak == '!'))
    {
      uart_receive_str_bak = uart_receive_str;
      uart1_get_ok = 1;
    }
    else if ((uart1_mode == 2) && (sbuf_bak == '!'))
    {
      uart1_get_ok = 1;
    }

    if (uart_receive_str.length() >= RECV_SIZE)
    {
      uart_receive_str = "";
    }
  }
}

// 串口中断 for ESP32
void serialEventEsp32()
{
  static char sbuf2_bak;
  // 判断是否有数据到来，有的话就全部读取(非阻塞)
  static unsigned long systick_ms_bak_esp32serial = 0;
  if (millis() - systick_ms_bak_esp32serial > MOTOR_PWM_SEND_PERIOD * 5)
  {
    systick_ms_bak_esp32serial = millis();
    while (Serial_Esp32.available())
    {
      sbuf2_bak = char(Serial_Esp32.read());

      // 如果接收完成，直接返回
      if (uart2_get_ok)
        return;

      // 根据不同模式处理接收到的数据
      if (uart2_mode == 0)
      {
        if (sbuf2_bak == '<')
        {
          uart2_mode = 4;
        }
        else if (sbuf2_bak == '{')
        {
          uart2_mode = 3;
        }
        else if (sbuf2_bak == '$')
        {
          uart2_mode = 1;
        }
        else if (sbuf2_bak == '#')
        {
          uart2_mode = 2;
        }
        uart2_receive_str = ""; // 清空接收字符串
      }

      // 累加接收到的数据
      uart2_receive_str += sbuf2_bak;
      // 依据结束条件判断是否接收完成
      if ((uart2_mode == 4 && sbuf2_bak == '>') ||
          (uart2_mode == 3 && sbuf2_bak == '}') ||
          (uart2_mode == 1 && sbuf2_bak == '!') ||
          (uart2_mode == 2 && sbuf2_bak == '!'))
      {
        uart2_get_ok = 1; // 设置接收完成标志
                          // 可以在这里处理接收到的字符串
      }

      // 限制接收缓冲区大小
      if (uart2_receive_str.length() >= RECV_SIZE)
      {
        uart2_receive_str = ""; // 重新清空
      }
    }
  }
}

// 解析串口指令
void parse_cmd2(u8 *uart_receive_buf)
{
  Serial.print("[PARSE_CMD2] ");
  Serial.println((char *)uart_receive_buf);
  u16 pos;
  int int1, int2, int3, int4;
  pos = str_contain_str((char *)uart_receive_buf, "$");
  if (pos = str_contain_str((char *)uart_receive_buf, "$SETPWM:"), pos)
  {
    // 解析速度指令 $SETPWM:1500,1500,1500,1500!
    if (sscanf((char *)uart_receive_buf, "$SETPWM:%d,%d,%d,%d!", &int1, &int2, &int3, &int4))
    {
      if (int1 >= MOTOR_PWM_MIN && int1 <= MOTOR_PWM_MAX && int2 >= MOTOR_PWM_MIN && int2 <= MOTOR_PWM_MAX && int3 >= MOTOR_PWM_MIN && int3 <= MOTOR_PWM_MAX && int4 >= MOTOR_PWM_MIN && int4 <= MOTOR_PWM_MAX)
      {
        set_motor_pwm(0, int1);
        set_motor_pwm(1, int2);
        set_motor_pwm(2, int3);
        set_motor_pwm(3, int4);
      }
    }
  }
  else if (pos = str_contain_str((char *)uart_receive_buf, "$SETTAG:"), pos)
  {
    // 解析设置目标速度指令 $SETTAG:1500,1500,1500,1500!
    if (sscanf((char *)uart_receive_buf, "$SETTAG:%d,%d,%d,%d!", &int1, &int2, &int3, &int4))
    {
      set_motor_tag_pulse(0, int1);
      set_motor_tag_pulse(1, int2);
      set_motor_tag_pulse(2, int3);
      set_motor_tag_pulse(3, int4);
    }
  }
  else if (pos = str_contain_str((char *)uart_receive_buf, "$GETTAGSPD"), pos)
  {
    // 解析获取目标速度指令 $GETTAGSPD!
    char response_msg[64];
    sprintf(response_msg, "<GETTAGSPD> %04d %04d %04d %04d@", motors_target[0], motors_target[1], motors_target[2], motors_target[3]);
    Serial_Esp32.println(response_msg);
  }
  else if (pos = str_contain_str((char *)uart_receive_buf, "$GETCURPWM"), pos)
  {
    // 解析获取目标速度指令 $GETCURPWM!
    char response_msg[64];
    sprintf(response_msg, "<GETCURPWM> %04d %04d %04d %04d@", motors_pwm[0], motors_pwm[1], motors_pwm[2], motors_pwm[3]);
    Serial_Esp32.println(response_msg);
  }
  else if (pos = str_contain_str((char *)uart_receive_buf, "$GETSPD"), pos)
  {
    // 解析获取当前速度指令 $GETSPD!
    char response_msg[64];
    sprintf(response_msg, "<GETSPD> %04d %04d %04d %04d@", motors_current[0], motors_current[1], motors_current[2], motors_current[3]);
    Serial_Esp32.println(response_msg);
  }
}

char cmd_return_tmp[64];

void setup()
{
  Serial.begin(115200); // 串口初始化
  Serial_Esp32.begin(9600);
  Serial_Esp32.listen();

  setup_nled();          // led灯闪烁初始化
  setup_beep();          // 蜂鸣器初始化
  setup_start_pre_cmd(); // 系统启动

  for (int i = 0; i < 3; i++)
  {
    beep_on();
    delay(100);
    beep_off();
    delay(100);
  }
}

void loop()
{
  loop_nled(); // 切换LED灯状态实现LED灯闪烁
  loop_uart(); // 解析串口接收到的字符串指令
  serialEventEsp32();
  loop_send_motor_pwm();
  if (downLoad)
    return;
}